#define MINDVISION
#ifdef MINDVISION

#include"ImageConsProd.h"
volatile unsigned int prdIdx;
volatile unsigned int csmIdx;
volatile unsigned int save_image_index;

//#define RECORD
//#define SPEED
//#define TIME
//#define DISP_RECEIVE
#define DISP_SEND

#ifdef TIME
double P_time_now;
double P_time_last;
double D_time_now;
double D_time_last;
#endif
#define BUFFER_SIZE 1   //图片缓冲区
struct ImageData {
    Mat img;
    unsigned int frame;
    unsigned int timestamp;
};
ImageData data[BUFFER_SIZE];

///////////////////串口///////////////////
SerialPort port("/dev/ttyUSB");    //ls /dev/
Data rdata;
int pre_mode = 0;
///////////////////相机///////////////////
GX_DEV_HANDLE hDevice = NULL;   //设备句柄

unsigned int last_time_stamp;
///////////////////OTHERS///////////////////
uint32_t time_nuc;    //当前时间
uint32_t time_gyro;
double last_time;   //上一次时间
uint32_t gyro_start_stamp;
double nuc_start_stamp;
VisionData vdata;


double cale_distance(Point a, Point b ){
    return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
}

int assert_success(GX_STATUS status)
{
    if (status != GX_STATUS_SUCCESS)
    {
        status = GXCloseDevice(hDevice);
        if (hDevice != NULL)
        {

        }
        status = GXCloseLib();
        return 0;
    }
}

//correct the received data
void correct_info(int &mode, int &shoot_speed, int &my_color,
                  double &gyro_yaw, double &gyro_pitch)
{
    if(mode!=1 or mode!=4)
    {
        mode=1;
    }
    shoot_speed=30;
    if(my_color!=1 and my_color!=2)
    {
        my_color=1;
    }
    if(gyro_yaw<-180)
    {
        gyro_yaw=-180;
    }
    else if(gyro_yaw>180)
    {
        gyro_yaw=180;
    }
    gyro_yaw+=180;
    if(gyro_pitch>180)
    {
        gyro_pitch=180;
    }
    else if(gyro_pitch<-90)
    {
        gyro_pitch=-180;
    }
}

//回调函数
static void GX_STDC OnFrameCallbackFun(GX_FRAME_CALLBACK_PARAM* pFrame)
{
#ifndef NO_PORT
    cout<<"///////////////////  prdIdx = "<<prdIdx<<"///////////////////"<<endl;
    //receive
    port.get_Mode(rdata.mode, rdata.shoot_speed, rdata.my_color, rdata.gyro_yaw, rdata.gyro_pitch, rdata.anti_flag,rdata.time_stamp);
#ifdef DISP_RECEIVE
    cout<<"get mode："<<rdata.mode<<" 颜色： "<<rdata.my_color<<" 射速： "<<rdata.shoot_speed<<" yaw: "<<rdata.gyro_yaw<<" pit: "<<rdata.gyro_pitch<<" anti_flag "<<rdata.anti_flag<<"time_stamp"<<rdata.time_stamp<<endl;
//    correct_info(rdata.mode, rdata.shoot_speed, rdata.my_color, rdata.gyro_yaw, rdata.gyro_pitch);
//    cout<<"corrected mode："<<rdata.mode<<" 颜色： "<<rdata.my_color<<" 射速： "<<rdata.shoot_speed<<" yaw: "<<rdata.gyro_yaw<<" pit: "<<rdata.gyro_pitch<<" anti_flag "<<rdata.anti_flag<<"time_stamp"<<rdata.time_stamp<<endl;
#endif DISP_RECEIVE
    pre_mode = rdata.mode;
#endif
    //get image
    int64_t nWidth=1280;
    int64_t nHeight=1024;
    uchar* m_pBufferRaw = new uchar[VIDEO_WIDTH * VIDEO_HEIGHT * 3]; ///< 原始图像数据（内存空间）
    uchar* pRGB24Buf = new uchar[nWidth * nHeight * 3];    ///< RGB图像数据（内存空间）


    if (pFrame->status == 0)                               // 正常帧，残帧返回-1
    {
        //更新nuc时间戳
        time_nuc = uint32_t((getTickCount()-nuc_start_stamp)*1000/getTickFrequency());
        time_gyro=rdata.time_stamp-gyro_start_stamp;

        Mat src;
        src.create(nHeight,nWidth, CV_8UC3);
        memcpy(m_pBufferRaw, pFrame->pImgBuf, pFrame->nImgSize);
        DxRaw8toRGB24(m_pBufferRaw, pRGB24Buf, (VxUint32)(pFrame->nWidth), (VxUint32)(pFrame->nHeight), RAW2RGB_NEIGHBOUR, DX_PIXEL_COLOR_FILTER(1), false);
        memcpy(src.data, pRGB24Buf, nHeight * nWidth * 3);

        double time_start = 0 , time_end = 0;
        time_start = getTickCount();
        while (prdIdx - csmIdx >= BUFFER_SIZE)
        {
            time_end = getTickCount();
            if((time_end - time_start)*1000/getTickFrequency()>2000 && prdIdx !=0 )
            {
                struct tm *newtime;
                char tmpbuf[128];
                time_t test;
                time(&test);
                newtime=localtime(&test);
                strftime(tmpbuf, 128, "%c", newtime);
                cout<<tmpbuf<<"取图超时，线程锁卡住"<<endl;
                close(-1);
                exit(-1);
            }
        }

        data[0].img = src.clone();
        if(data[0].img.empty())
        {
            return ;
        }
        double t2 = getTickCount();
        cout<<"FPS"<<1000/((t2-last_time)*1000/getTickFrequency())<<endl;
        last_time=t2;
        GXFlushQueue(hDevice);  //
        prdIdx++;

    }
    delete[] m_pBufferRaw;
    delete[] pRGB24Buf;
    return;
}

void ImageConsProd::ImageProducer()
{
#ifndef NO_PORT
    cout<<"begin to init serial port"<<endl;
    port.initSerialPort();
    waitKey(200);//等待，否则收到数据为0
    port.get_Mode(rdata.mode, rdata.shoot_speed, rdata.my_color, rdata.gyro_yaw, rdata.gyro_pitch, rdata.anti_flag,rdata.time_stamp);

    gyro_start_stamp=rdata.time_stamp;
    nuc_start_stamp=getTickCount();
#endif
    rdata.mode=1;
    rdata.shoot_speed=30;
    rdata.my_color=2; //1:hit red  2:hit blue
    rdata.gyro_yaw=0;
    rdata.gyro_pitch=0;
    rdata.anti_flag=0;
    // 主相机初始化
    GX_STATUS status = GX_STATUS_SUCCESS;
    GX_OPEN_PARAM stOpenParam;
    uint32_t nDeviceNum = 0;

    cout << "主相机初始化中......" << endl;
    status = GXCloseLib();
    status = GXInitLib();
    if (status != GX_STATUS_SUCCESS)
    {
        cout << "camera init failed!" << endl;
        return ;
    }
    cout << "camera init success !" << endl;

    status = GXUpdateDeviceList(&nDeviceNum, 1000);
    if ((status != GX_STATUS_SUCCESS) || (nDeviceNum <= 0))
    {
        cout << "获取设备列表失败" << endl;
        return ;
    }
    cout << "设备数：" << nDeviceNum << endl;

    stOpenParam.accessMode = GX_ACCESS_EXCLUSIVE;
    stOpenParam.openMode = GX_OPEN_INDEX;
    stOpenParam.pszContent = "1";
    status = GXOpenDevice(&stOpenParam, &hDevice);

    if (status == GX_STATUS_SUCCESS)
    {
        //设置采集模式为连续采集
        status = GXSetEnum(hDevice, GX_ENUM_ACQUISITION_MODE, GX_ACQ_MODE_CONTINUOUS);
        assert_success(status);

        if (rdata.my_color==1)
        {
            status = GXSetFloat(hDevice, GX_FLOAT_EXPOSURE_TIME, 3500);
        }
        else if(rdata.my_color==2)
        {
            status = GXSetFloat(hDevice, GX_FLOAT_EXPOSURE_TIME, 3500);
        }
        if (status == GX_STATUS_SUCCESS)
        {
            cout << "设置曝光值：" << endl;
            assert_success(status);
        }
        else
            cout << "SET_EXPOSURE_FAIL" << endl;


        status = GXSetInt(hDevice, GX_INT_HEIGHT, 1024);
        status = GXSetInt(hDevice, GX_INT_WIDTH,1280);
//            status = GXSetInt(hDevice, GX_INT_OFFSET_X, 100);
//        status = GXSetInt(hDevice, GX_INT_OFFSET_Y, 0);
        assert_success(status);

        //GAIN_SET
        status = GXSetEnum(hDevice, GX_ENUM_GAIN_SELECTOR, GX_GAIN_SELECTOR_ALL);
        status = GXSetFloat(hDevice, GX_FLOAT_GAIN, 0);
        if (status == GX_STATUS_SUCCESS){
            cout << "set_gain_succeed_ggg " << endl;
            assert_success(status);
        }else{
            cout <<"增益 fail:  "  << status <<endl;
        }

        //white balance
        status = GXSetEnum(hDevice, GX_ENUM_BALANCE_RATIO_SELECTOR, GX_BALANCE_RATIO_SELECTOR_RED);
        status = GXSetFloat(hDevice, GX_FLOAT_BALANCE_RATIO, 1.56);
        status = GXSetEnum(hDevice, GX_ENUM_BALANCE_RATIO_SELECTOR, GX_BALANCE_RATIO_SELECTOR_GREEN);
        status = GXSetFloat(hDevice, GX_FLOAT_BALANCE_RATIO, 1);
        status = GXSetEnum(hDevice, GX_ENUM_BALANCE_RATIO_SELECTOR, GX_BALANCE_RATIO_SELECTOR_BLUE);
        status = GXSetFloat(hDevice, GX_FLOAT_BALANCE_RATIO, 1.56);
        if (status == GX_STATUS_SUCCESS){
            cout <<"set_whitebalance_succeed_ggg " <<endl;
            assert_success(status);
        }else{
            cout <<"whitebalance fail:  "  << status <<endl;
        }


        //设置触发开关为OFF
        status = GXSetEnum(hDevice, GX_ENUM_TRIGGER_MODE, GX_TRIGGER_MODE_OFF);
        assert_success(status);

        //注册图像处理回调函数
        status = GXRegisterCaptureCallback(hDevice, NULL, OnFrameCallbackFun);
        cout << "回调函数:" << status << endl;

        //发送开采命令
        GXSendCommand(hDevice, GX_COMMAND_ACQUISITION_START);
        while(1);

        //发送停采命令
        status = GXSendCommand(hDevice, GX_COMMAND_ACQUISITION_STOP);
        //注销采集回调
        status = GXUnregisterCaptureCallback(hDevice);
    }

    //在结束的时候调用GXCLoseLib()释放资源
    status = GXCloseDevice(hDevice);
    status = GXCloseLib();
}

void ImageConsProd::ImageConsumer()
{
    ArmorDetector armordetector;
    my_predict my_pre;
    int cons_pre_mode = 0;
    int current_mode = 0;
    int pic_num=0;

    CShareMemory draw_line1("txh", 128);
    CShareMemory draw_line2("txh2", 128);

#ifdef RECORD
    ofstream pnp_dis("pnp_dis.txt");
#endif
    armordetector.center_x=703;
    armordetector.center_y=507;
    // 相机内参
    Mat cam_intri = (Mat_<double>(3, 3) << 1728.73637095202,	0,	armordetector.center_x,
                                            0,	1729.48629868933,	armordetector.center_y,
                                            0,	0,	1);

    // 畸变系数
    Mat distortion = (Mat_<double>(5, 1) << -0.1156, 0.4920, -0.7336,
                                    -0.007, 0.0022);


    while (1){
        while(prdIdx - csmIdx == 0)
        {
        }
//        cout<<"******csmIdx= "<<csmIdx<<"********************"<<endl;

//rdata.my_color=1;
        vdata._mode = rdata.mode;

        current_mode = rdata.mode;
        Mat src = data[csmIdx % BUFFER_SIZE].img.clone();
        csmIdx++;

        if(src.empty())
        {
            cout<<"src is empty"<<endl;
            vdata.reset(time_nuc);
            continue;
        }

        if(rdata.mode == 0)
        {
            armordetector.ctrl_reset();
            vdata.reset(time_nuc);
#ifdef  TAKE_PHOTO
            namedWindow("src",0);
            imshow("src",src);
            if(waitKey(20)=='t')
            {
                char pic_name[128]={};
                sprintf(pic_name,"pictures/%d.jpg",pic_num);
                cout<<pic_name<<endl;
                imwrite(pic_name,src);
                pic_num++;
            }
#endif
        }
        if(current_mode == 1 || current_mode == 4)
        {
            double light_len;
//            armordetector.src = src.clone();
            armordetector.src = src;
            armordetector.armor_ctrl(1, cam_intri, distortion, rdata);
            armordetector.info_src(rdata);

            my_pre.init_coordinate();

            int drop_flag = 0;
            if(armordetector.drop_frame_cnt <= 20 && armordetector.drop_frame_cnt > 0)
            {
                armordetector.target_armor.empty = 0;
                drop_flag = 1;
            }
            if(armordetector.target_armor.empty)
            {
                vdata.reset(time_nuc);
                armordetector.target_armor.armor_reset();
                armordetector.pre_target.armor_reset();
                my_pre.reset_predictor();

            }
            else
            {
                if(!drop_flag)
                {
                    my_pre.update_abs_info(armordetector.target_armor.pitch,armordetector.target_armor.yaw);
                }

                my_pre.predict_next_pos(armordetector.drop_frame_cnt);


////                armordetector.set_offset(armordetector.target_armor.distance , mf.pit_out, mf.yaw_out, rdata.shoot_speed, rdata.mode, rdata.my_color);
//                light_len = MAX(cale_distance(armordetector.target_armor.right_top, armordetector.target_armor.right_bottom),cale_distance(armordetector.target_armor.left_top, armordetector.target_armor.left_bottom) );


                draw_line1.Data2Memory(my_pre.yaw_out);  //blue
                draw_line2.Data2Memory(armordetector.target_armor.yaw);  //orange


//                vdata.yaw_angle.f = -predictor.yaw_out ;
//                vdata.pitch_angle.f = -predictor.pit_out;

                vdata.yaw_angle.f = -armordetector.target_armor.yaw ;
                vdata.pitch_angle.f = -armordetector.target_armor.pitch;
                vdata.dis.f = armordetector.target_armor.distance;
                vdata.isFindTarget = 1;
                vdata.drop_frame = 0;
                vdata.isfindDafu = 0;
                vdata.anti_top = 0;
                vdata.anti_top_change_armor = 0;
                vdata.time_stamp=time_nuc;

#ifdef RECORD

                pnp_dis << endl << armordetector.pnp_dis;
#endif
            }
        }


        cout<<"vdata.time_stamp:"<<vdata.time_stamp<<endl;
        cons_pre_mode = current_mode;
        vdata.limit_data();
//        all_vdata.push_back(vdata);
#ifndef NO_PORT
//        VisionData send_data= match_vdata(time_gyro);
//        draw_line1.Data2Memory(int(time_nuc)-int(time_gyro));
//        draw_line2.Data2Memory(-(int(send_data.time_stamp)-int(time_gyro)));



        port.TransformData(vdata);
        port.send();
        if(port.result == -1)
        {
            struct tm *newtime;
            char tmpbuf[128];
            time_t test;
            time(&test);
            newtime=localtime(&test);
            strftime(tmpbuf, 128, "%c", newtime);
            cout<<tmpbuf<<endl;
            cout<<"串口发送失败，自动重启"<<endl;
            port.closePort();
            port.reinitSerialPort("/dev/ttyUSB");
        }
#endif
    }
#ifdef RECORD
    pnp_dis.close();
#endif
    port.closePort();
}

VisionData ImageConsProd::match_vdata(uint32_t time_gyro)
{
    cout<<"matching data -----------------------------------------"<<endl;
    cout<<"time_gyro: "<<time_gyro<<endl;
    int min_idx=0;
    int min_differ=999999;

    for(int i=0;i<all_vdata.size();++i)
    {
        int time_d=int(all_vdata[i].time_stamp)-int(time_gyro);
        cout<<time_d<<endl;
        //时间戳匹配
        if( abs(time_d)<min_differ)
        {
            min_differ=abs(time_d);
            min_idx=i;
        }
        else
        {
            break;
        }
    }
    all_vdata.erase(all_vdata.begin(),all_vdata.begin()+min_idx);
    return all_vdata[0];
}

void ImageConsProd::ImageCapture()
{
    cout << " ------ IMAGE WRITE TASK ON !!! ------" << endl;
    SaveVideo writer;
    while(writer.getState()){
        while(static_cast<int>(prdIdx - save_image_index) <= 0);
        Mat img_tmp;
        if(data[csmIdx % BUFFER_SIZE].img.empty())
        {
            cout << "debug"<<endl;
            continue;
        }
        data[csmIdx % BUFFER_SIZE].img.copyTo(img_tmp);
        if(img_tmp.empty())
        {
            struct tm *newtime;
            char tmpbuf[128];
            time_t test;
            time(&test);
            newtime=localtime(&test);
            strftime(tmpbuf, 128, "%c", newtime);
            cout<<tmpbuf<<endl;
            cout<<"IMAGE EMPTY"<<endl;
            continue;
        }
        if(img_tmp.cols != 1280 or img_tmp.rows!=1024)
        {
            cv::resize(img_tmp, img_tmp, cv::Size(1280,1024));
        }
        writer.updateImage(img_tmp);
        save_image_index++;
    }
}

void ImageConsProd::ImageGetinfo()
{
    while(1)
    {
        usleep(1);
//        port.get_Mode(rdata.mode, rdata.shoot_speed, rdata.my_color, rdata.gyro_yaw, rdata.gyro_pitch);
        cout<<"模式："<<rdata.mode<<" 颜色： "<<rdata.my_color<<" 射速： "<<rdata.shoot_speed<<" yaw: "<<rdata.gyro_yaw<<" pit: "<<rdata.gyro_pitch<<endl;
    }

}

#endif
